module TowerRotateP {
	provides interface TowerRotate;
	provides interface Init;

	uses  {
		interface MotorControl;
		interface HplAtm128Interrupt as DivisionInt; // fogaskerek erzekelo
		interface GeneralIO as TowerSidePin; // jobb/bal erzekelo
	
		interface Timer<TMilli>;
		interface Leds;
	}
}

implementation {

	enum {
		IDLE = 0,
		TURN_CENTER,
		TURN_ST,
		TURN_REL,
		TURN_POS		
	};

	enum {
		MAXPOS = 45,
		MINPOS = -MAXPOS
	};	
	uint8_t state = IDLE;
	int16_t heading = 0;
	int16_t desired;

	int8_t dir = 1;
	bool leftside = FALSE;
	bool turnactive = FALSE;

	command error_t Init.init() {
		call TowerSidePin.makeInput();
		call DivisionInt.disable();
		call DivisionInt.clear();
		call DivisionInt.edge(TRUE);	
		call DivisionInt.enable();
		leftside = call TowerSidePin.get();
		return SUCCESS;
	}
	
	command int16_t TowerRotate.getPos() {
		int16_t pos_;
		atomic {pos_ = heading;}
		return pos_;
	}

	command error_t TowerRotate.turnCenter(int8_t speed) {
		atomic {
			if (state != IDLE)
				return FAIL;
			state = TURN_CENTER;
		}
		leftside = call TowerSidePin.get();
		if (leftside)
			call MotorControl.setSpeed(((int16_t)(-speed))<<8);
		else
			call MotorControl.setSpeed(((int16_t)(speed))<<8);
		call Timer.startPeriodic(20);
		// stop at center		
	}
	
	command error_t TowerRotate.turnST(int8_t speed, uint16_t time) {
		atomic {
			if (state != IDLE)
				return FAIL;
			state = TURN_ST;
		}
//		call Leds.set(0xa1);
		if (speed > 0)
			dir = 1;
		else if (speed < 0)
			dir = -1;
		else {
			dir = 0;
			atomic state = IDLE;
			//TODO: post done?
			return SUCCESS;
		}
		call MotorControl.setSpeed(((uint16_t)speed)<<8);
		call Timer.startOneShot(time);
		return SUCCESS;
	}
	
	command error_t TowerRotate.turnPos(int8_t speed, int16_t position) {
		atomic {
			if (state != IDLE)
				return FAIL;
			state = TURN_POS;
		}				
//		call Leds.set(0xa2);
		atomic {
			desired = position;
			if (desired < MINPOS)
				desired = MINPOS;
			else if (desired > MAXPOS)
				desired = MAXPOS;
			if (desired > heading) {
				dir = 1;
				call MotorControl.setSpeed(((int16_t)speed)<<8);
			} else if (desired < heading) {
				dir = -1;
				call MotorControl.setSpeed(((int16_t)-speed)<<8);
			} else {
				state = IDLE;
				//TODO: post done?
				return SUCCESS;
			}
		}
		call Timer.startOneShot(2000); // timeout for bad sensor
		return SUCCESS;
	}
	
	command error_t TowerRotate.turnRelPos(int8_t speed, int16_t turn) {
//		call Leds.set(0xa3);
		atomic {
			if (state != IDLE)
				return FAIL;
			state = TURN_REL;
			desired = heading + turn;
			if (desired < MINPOS)
				desired = MINPOS;
			else if (desired > MAXPOS)
				desired = MAXPOS;
			if (desired > heading) {
				dir = 1;
				call MotorControl.setSpeed(((int16_t)speed)<<8);
			} else if (desired < heading) {
				dir = -1;
				call MotorControl.setSpeed(((int16_t)-speed)<<8);
			} else {
				state = IDLE;
				//TODO: post done?
				return SUCCESS;
			}
			call Timer.startOneShot(2000);
		}
		return SUCCESS;
	}
	
	async event void DivisionInt.fired() {
//		call DivisionInt.disable();
//		call DivisionInt.clear();
//		call DivisionInt.edge(!call DivisionInt.getValue());
//		call DivisionInt.enable();
		atomic {
			heading +=dir;
			if ((leftside) != (call TowerSidePin.get())) {
				leftside = !leftside;
				heading = leftside ? 0 : -1;
			}
			turnactive = TRUE;
			if (state == TURN_REL || state == TURN_POS) {
//			call Leds.set(heading & 0xFF);
				if (heading == desired) {
					call MotorControl.setSpeed(0);
					call Timer.stop();
					//TODO:signal event
					
					//dir = 0;
					state = IDLE;
				} //else
					//call Timer.startOneShot(2000); // restart timeout (vs. turnactive?)
				
			}
		}
		call Leds.led2Toggle();
	}

	event void Timer.fired() {
		atomic {
			if (state == TURN_CENTER) {
				if (leftside != call TowerSidePin.get()) {
					leftside = !leftside;
					heading = leftside ? 0 : -1;
					call Timer.stop();
					call MotorControl.setSpeed(0);
					state = IDLE;
				}
			} else if (!turnactive || state == TURN_ST) {
				call MotorControl.setSpeed(0);
				state = IDLE;
				//TODO: signal event
			} else {
				turnactive = FALSE;
				call Timer.startOneShot(2000);
			}
		}
	}
}
